/*
* Copyright (c) 2007 by Kirill Kolodyazhniy.
* See the file "license.terms" for information on usage and redistribution.
*/

#include "odesolid.h"
#include "odeutil.h"
#include "odespace.h"

namespace Engine
{
	/***************************************************************************************************************/
	OdeSolid::OdeSolid(dWorldID worldID, dSpaceID spaceID, const std::string& name, SceneNodePtr node)
		:Solid(name, node)
		,m_worldID(worldID)
		,m_spaceID(spaceID)
		,m_enabled(true)
		,m_static(false)
		,m_gravityEnabled(true)
		,m_transform(Matrix4<REAL>::IDENTITY)
	{
		m_bodyID = dBodyCreate(m_worldID);
		dBodySetAutoDisableDefaults(m_bodyID);
		dBodySetData(m_bodyID,this);
	}
	/***************************************************************************************************************/
	OdeSolid::~OdeSolid()
	{
		DestroyGeoms();

		if (!IsStatic())
		{
			// This is a dynamic solid, so it has an ODE body that needs to be destroyed.
			dBodyDestroy(m_bodyID);
		}
	}
	/***************************************************************************************************************/
	void OdeSolid::DestroyGeoms()
	{
		GeomVector::iterator i = m_geoms.begin(), e = m_geoms.end();

		for (;i != e; ++i)
		{
			if (0 != i->transformID)
				dGeomDestroy(i->transformID);

			if (0 != i->trimeshDataID)
				dGeomTriMeshDataDestroy(i->trimeshDataID);

			// Destroy the ODE geom.
			if(0 != i->geomID)
				dGeomDestroy(i->geomID);
		}
		m_geoms.clear();
	}
	/***************************************************************************************************************/
	bool OdeSolid::IsEnabled() const
	{
		return m_enabled;
	}
	/***************************************************************************************************************/
	void OdeSolid::SetEnabled(bool e)
	{
		m_enabled = e;
		if (e)
		{
			// If this Solid is dynamic, enable the ODE body.
			if (!IsStatic())
				dBodyEnable(m_bodyID);

			// Enable the ODE geoms.
			GeomVector::iterator i = m_geoms.begin(), e = m_geoms.end();
			for (;i != e; ++i)
				dGeomEnable(i->geomID);

		}
		else // Disable the Solid.
		{
			// If this Solid is dynamic, disable the ODE body.
			if (!IsStatic())
				dBodyDisable(m_bodyID);

			// Disable the ODE geoms.
			GeomVector::iterator i = m_geoms.begin(), e = m_geoms.end();
			for (;i != e; ++i)
				dGeomDisable(i->geomID);
		}
	}
	/***************************************************************************************************************/
	bool OdeSolid::IsStatic() const
	{
		return m_static;
	}
	/***************************************************************************************************************/
	void OdeSolid::SetStatic(bool s)
	{
		if (true == IsStatic())
		{
			if (false == s)
			{
				// Make this object dynamic.
				m_static = false;

				// Create a new body.
				m_bodyID = dBodyCreate(m_worldID);


				// Set the position of the new body.
				dMatrix3 r;
				dReal x,y,z;
				OdeUtil::Convert(m_transform, &r, &x, &y, &z);
				dBodySetRotation(m_bodyID, r);
				dBodySetPosition(m_bodyID, x, y, z);

				// Loop over the geoms.
				GeomVector::iterator i = m_geoms.begin(), b = m_geoms.begin(), e = m_geoms.end();
				for (;i != e; ++i)
				{
					dMass newMass;

					// Get a pointer to this geom's ShapeData.
					const ShapeData* shapeData = i->GetShapeData();

					// Setup mass.
					switch (shapeData->type)
					{
						case ST_BOX:
							{
								const BoxShapeData * boxData = static_cast<const BoxShapeData*>(shapeData);

								if(!boxData->material.IsNull())
								{
									dMassSetBox(&newMass,
								             static_cast<dReal>(boxData->material->density),
											 static_cast<dReal>(boxData->size.x),
								             static_cast<dReal>(boxData->size.y),
								             static_cast<dReal>(boxData->size.z));
								}
								break;
							}
						case ST_SPHERE:
							{
								const SphereShapeData* sphereData =  static_cast<const SphereShapeData*>(shapeData);
								if(!sphereData->material.IsNull())
								{
									dMassSetSphere(&newMass,
								                static_cast<dReal>(shapeData->material->density),
								                static_cast<dReal>(sphereData->radius));
								}
								break;
							}
						case ST_MESH:
							if(!shapeData->material.IsNull())
							{
								dMassSetTrimesh(&newMass, static_cast<dReal>(shapeData->material->density), i->geomID);
							}
							break;
					}


					// Setup the new mass.
					AddMass(&newMass);

					// Add each geom to the new body.
					if (0 == i->transformID)
					{
						// No geom transform.
						dGeomSetBody(i->geomID, m_bodyID);
					}
					else
					{
						// Use geom transform.
						dGeomSetBody(i->transformID, m_bodyID);
					}
				}

				MoveToSpace();
			}
			else
			{
				// do nothing
			}
		}
		else // this object is not static
		{
			if (true == s)
			{
				// make this object static
				m_static = true;

				// destroy the body
				dBodyDestroy(m_bodyID);

				MoveToSpace();
			}
			else
			{
				// do nothing
			}
		}
	}
	/***************************************************************************************************************/
	bool OdeSolid::IsGravityEnabled()const
	{
		return m_gravityEnabled;
	}
	/***************************************************************************************************************/
	void OdeSolid::SetGravityEnabled(bool e)
	{
		m_gravityEnabled = e;

		if (e)
		{
			dBodySetGravityMode(m_bodyID, 1);
		}
		else
		{
			dBodySetGravityMode(m_bodyID, 0);
		}
	}
	/***************************************************************************************************************/
	void OdeSolid::SetSpace(SpacePtr space)
	{
		// update solid's space which will be used for future shapes
		m_spaceID = static_cast<OdeSpace*>(space.GetPointer())->GetID();

		MoveToSpace();
	}
	/***************************************************************************************************************/
	void OdeSolid::ZeroForcesImpl()
	{
		if (!IsStatic())
		{
			dBodySetForce(m_bodyID, 0, 0, 0);
			dBodySetTorque(m_bodyID, 0, 0, 0);
		}
	}
	/***************************************************************************************************************/
	void OdeSolid::SetMass(const Mass& newmass)
	{
		if (IsStatic())
			return;

		dMass newMass;
		newMass.setParameters(static_cast<dReal>(newmass.mass), 0, 0, 0,
			static_cast<dReal>(newmass.inertia[0][0]), static_cast<dReal>(newmass.inertia[1][1]), static_cast<dReal>(newmass.inertia[2][2]),
			static_cast<dReal>(newmass.inertia[0][1]), static_cast<dReal>(newmass.inertia[0][2]), static_cast<dReal>(newmass.inertia[1][2]));

		dBodySetMass(m_bodyID, &newMass);
	}
	/***************************************************************************************************************/
	void OdeSolid::AddMass(dMass* newMass)
	{
		if (IsStatic())
			return ;

		if (m_geoms.empty())
		{
			dBodySetMass(m_bodyID, newMass);
		}
		else
		{
			dMass totalMass;
			dBodyGetMass(m_bodyID, &totalMass);
			dMassAdd(&totalMass, newMass);
			dBodySetMass(m_bodyID, &totalMass);
		}

	}
	/***************************************************************************************************************/
	REAL OdeSolid::GetMass() const
	{
		if (IsStatic())
		{
			return 0;
		}
		else
		{
			dMass mass;
			dBodyGetMass(m_bodyID, &mass);
			return static_cast<REAL>(mass.mass);
		}
	}
	/***************************************************************************************************************/
	void OdeSolid::ApplyForce(const Force& f)
	{
		switch (f.type)
		{
			case FT_FORCE:
				dBodyAddForce(m_bodyID, static_cast<dReal>(f.vec.x), static_cast<dReal>(f.vec.y), static_cast<dReal>(f.vec.z));
				break;
			case FT_TORQUE:
				dBodyAddTorque(m_bodyID, static_cast<dReal>(f.vec.x), static_cast<dReal>(f.vec.y), static_cast<dReal>(f.vec.z));
				break;
		}
	}
	/***************************************************************************************************************/
	void OdeSolid::AddShapeImpl(const ShapeData& shape)
	{
		dGeomID newGeomID = NULL;
		dGeomID newTransformID = NULL;
		dTriMeshDataID newTrimeshDataID = NULL;
		dSpaceID spaceID = NULL;
		dMass newMass;

		if (Matrix4<REAL>::IDENTITY == shape.position)
		{
			// No offset transform.
			spaceID = m_spaceID;
			newTransformID = 0;
		}
		else
		{
			// Use ODE's geom transform object.
			spaceID = 0;
			newTransformID = dCreateGeomTransform(m_spaceID);
		}

		GeomData geom;
		geom.SetShapeData(shape);

		switch (geom.GetShapeData()->type)
		{
			case ST_BOX:
				{
					BoxShapeData* boxData = static_cast<BoxShapeData*>(geom.GetShapeData());
					newGeomID = dCreateBox(spaceID,
					                       static_cast<dReal>( boxData->size.x),
					                       static_cast<dReal>( boxData->size.y),
					                       static_cast<dReal>( boxData->size.z));

					if(!boxData->material.IsNull())
					{
						dMassSetBox(&newMass, static_cast<dReal>(boxData->material->density),
											  static_cast<dReal>( boxData->size.x),
											  static_cast<dReal>( boxData->size.y),
											  static_cast<dReal>( boxData->size.z));
					}
					break;
				}
			case ST_SPHERE:
				{
					SphereShapeData* sphereData = static_cast<SphereShapeData*>(geom.GetShapeData());
					newGeomID = dCreateSphere(spaceID,
					                           static_cast<dReal>(sphereData->radius));

					if(!sphereData->material.IsNull())
					{
						dMassSetSphere(&newMass, static_cast<dReal>(sphereData->material->density),
												 static_cast<dReal>(sphereData->radius));
					}
					break;
				}
			case ST_MESH:
				{
					MeshShapeData* meshData = static_cast<MeshShapeData*>(geom.GetShapeData());
					meshData->Build();


					newTrimeshDataID = dGeomTriMeshDataCreate();

					dGeomTriMeshDataBuildSingle(newTrimeshDataID,
						static_cast<const void*>(&meshData->data->vertices[0]), meshData->data->vertexStride,
						meshData->data->vertices.size(), static_cast<const void*>(&meshData->data->indices[0]),
						meshData->data->indices.size(), meshData->data->triStride);

					newGeomID = dCreateTriMesh(spaceID, newTrimeshDataID, NULL, NULL, NULL);

					if(!meshData->material.IsNull())
						dMassSetTrimesh(&newMass, static_cast<dReal>(meshData->material->density), newGeomID);
					break;
				}
		}

		// This will do nothing if this is a static Solid.
		AddMass(&newMass);

		// Setup the geom.
		geom.geomID = newGeomID;
		geom.transformID = newTransformID;
		geom.spaceID = spaceID;
		geom.trimeshDataID = newTrimeshDataID;


		SetupNewGeom(geom);
	}
	/***************************************************************************************************************/
	void OdeSolid::SetupNewGeom(const GeomData& geom)
	{
		if (!IsStatic())
		{
			if (0 == geom.transformID)
			{
				// No geom transform.
				dGeomSetBody(geom.geomID, m_bodyID);
			}
			else
			{
				// Use geom transform.
				dGeomSetBody(geom.transformID, m_bodyID);
			}
		}

		if (0 != geom.transformID)
		{
			// Setup geom transform.
			dGeomTransformSetGeom(geom.transformID, geom.geomID);
			dMatrix3 r;
			dReal x, y, z;
			OdeUtil::Convert(geom.GetShapeData()->position,&r, &x, &y, &z);
			dGeomSetRotation(geom.geomID, r);
			dGeomSetPosition(geom.geomID, x, y, z);
		}

		// Store the GeomData
		m_geoms.push_back(geom);

		// Set the GeomData reference for later use (e.g. in collision handling).
		if (0 == geom.transformID)
		{
			// No geom transform.
			dGeomSetData(geom.geomID, &m_geoms.back());
		}
		else
		{
			// Using geom transform.
			dGeomSetData(geom.transformID, &m_geoms.back());
		}
	}
	/***************************************************************************************************************/
	Matrix4<REAL> OdeSolid::GetTransformImpl() const
	{
		Matrix4<REAL> rez;
		if (!IsStatic())
		{
			const dReal* pos = dBodyGetPosition(m_bodyID);
			const dReal* rot = dBodyGetRotation(m_bodyID);

			OdeUtil::Convert(rot,pos,&rez);
		}
		else
		{
			//average position from geoms
			GeomVector::const_iterator i = m_geoms.begin(), b = m_geoms.begin(), e = m_geoms.end();
			for (;i != e; ++i)
			{
				const dReal* pos = 0;
				const dReal* rot = 0;
				Matrix4<REAL> m;
				if (0 == i->transformID)
				{
					// No geom transform.
					rot = dGeomGetRotation(i->geomID);
					pos = dGeomGetPosition(i->geomID);
					OdeUtil::Convert(rot,pos,&m);
				}
				else
				{
					// Using geom transform.
					rot = dGeomGetRotation(i->transformID);
					pos = dGeomGetPosition(i->transformID);
					OdeUtil::Convert(rot,pos,&m);
				}
				rez = rez + m;
			}
			rez = rez / m_geoms.size();
		}

		return rez;
	}
	/***************************************************************************************************************/
	void OdeSolid::SetTransformImpl(const Matrix4<REAL>& t)
	{
		dMatrix3 r;
		dReal x, y, z;
		OdeUtil::Convert(t, &r, &x,& y, &z);

		m_transform = t;

		if (!IsStatic())
		{
			dBodySetRotation(m_bodyID, r);
			dBodySetPosition(m_bodyID, x, y, z);
		}
		else
		{
			GeomVector::iterator i = m_geoms.begin(), b = m_geoms.begin(), e = m_geoms.end();
			for (;i != e; ++i)
			{
				if (0 == i->transformID)
				{
					// No geom transform.
					dGeomSetRotation(i->geomID, r);
					dGeomSetPosition(i->geomID, x, y, z);
				}
				else
				{
					// Using geom transform.
					dGeomSetRotation(i->transformID, r);
					dGeomSetPosition(i->transformID, x, y, z);
				}
			}
		}
	}
	/***************************************************************************************************************/
	void OdeSolid::MoveToSpace()
	{
		// remove all current shapes from their spaces and add them to the
		// new one
		GeomVector::iterator i = m_geoms.begin(), b = m_geoms.begin(), e = m_geoms.end();
		for (;i != e; ++i)
		{
			if (0 != i->transformID)
			{
				// This geom uses a transform, so apply the new space only
				// to the transform geom.
				dSpaceRemove(i->spaceID, i->transformID);
				dSpaceAdd(m_spaceID, i->transformID);
			}
			else
			{
				// Normal geom with no transform.
				dSpaceRemove(i->spaceID, i->geomID);
				dSpaceAdd(m_spaceID, i->geomID);
			}

			i->spaceID = m_spaceID;
		}
	}
	/***************************************************************************************************************/
}
